#include "com/send_message_com.hpp"
#include "mavlink/common/mavlink.h"

SendMessageCom::SendMessageCom(QObject *parent, QSerialPort *serial)
    : QObject(parent) {
  this->serial = serial;
}

void SendMessageCom::sendMessage(QString message) {
  if (this->serial && this->serial->isOpen()) {
    serial->write(message.toUtf8());
    qDebug() << "send message: " << message;
  } else {
    qDebug() << "please connect com";
  }
}

int SendMessageCom::sendHeartbeat() {
  // mavlink_message_t是一个结构体,包含了消息ID、payload等字段
  // 在这里不需要显式初始化,因为:
  // 1. mavlink_msg_heartbeat_pack函数会完全重写msg的所有字段
  // 2. msg是局部变量,在栈上分配,C++会自动将其内存置为0
  mavlink_message_t msg;
  uint8_t system_id = 1;
  uint8_t component_id = 1;
  // 打包HEARTBEAT消息
  mavlink_msg_heartbeat_pack(system_id, component_id, &msg, MAV_TYPE_QUADROTOR,
                             MAV_AUTOPILOT_PX4, MAV_MODE_FLAG_SAFETY_ARMED, 0,
                             MAV_STATE_ACTIVE);
  return this->sendMavlinkMessage(&msg);
}

/**
 * 使用Qt发送MAVLink消息到串口
 * @param msg 已打包的MAVLink消息
 * @return 成功返回发送的字节数，失败返回-1
 *
 * 此处msg参数是必需的，因为:
 * 1. msg包含了完整的MAVLink消息内容，包括消息ID、payload等
 * 2. mavlink_msg_to_send_buffer函数需要msg作为输入来序列化消息
 * 3. 这个函数是通用的发送接口，用于发送各种类型的MAVLink消息
 * 4. 调用方负责打包具体的消息内容到msg中，此函数只负责发送
 */
int SendMessageCom::sendMavlinkMessage(mavlink_message_t *msg) {
  if (!this->serial || !this->serial->isOpen()) {
    qDebug() << "串口未初始化或未打开，发送失败";
    return -1;
  }
  // 序列化消息
  // buffer用于存储序列化后的MAVLink消息字节流
  // MAVLINK_MAX_PACKET_LEN是MAVLink协议定义的最大消息长度
  uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
  uint16_t len = mavlink_msg_to_send_buffer(buffer, msg);
  // 发送字节流到串口
  qint64 bytesSent = this->serial->write((const char *)buffer, len);
  if (bytesSent > 0) {
    qDebug() << "发送成功，字节数：" << bytesSent;
    return (int)bytesSent;
  } else {
    qDebug() << "消息发送失败：" << this->serial->errorString();
    return -1;
  }
}